home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Language/OS - Multiplatform Resource Library
/
LANGUAGE OS.iso
/
a_utils
/
expanded.lha
/
expanded
/
src
/
Matrix.C
< prev
next >
Wrap
C/C++ Source or Header
|
1992-03-19
|
28KB
|
1,013 lines
//
// Linear-Affine-Projective Geometry Package
//
// Matrix.C
//
// $Header$
//
// Austin Dahl and William J.R. Longabaugh
// University of Washington
//
// Copyright (c) 1990, 1991, 1992 Austin Dahl and William J.R. Longabaugh
// Copying, use and development for non-commercial purposes permitted.
// All rights for commercial use reserved.
// This software is unsupported and without warranty; it is
// provided "as is".
//
// Original implementation by Austin Dahl. Modified by WJRL to eliminate
// fixed maximum size and to simplify operations on identity matrices.
// Also modified to use routines based on algorithms described in
// "Numerical Recipes in C" by Press et. al. for matrix inversion
// and determinants.
//
// ***********************************************************************
#include <math.h>
#include <malloc.h>
#include <stream.h>
#include "Matrix.h"
#define OPENBRACKET_CHAR '{'
#define OPENBRACKET_STRING "{"
#define CLOSEBRACKET_CHAR '}'
#define CLOSEBRACKET_STRING "}"
#define COMMA_CHAR ','
#define SPACE_CHAR ' '
#define NULL_CHAR '\0'
// Function prototypes for local functions for doing LU decomposition
// and subsequent matrix inversion:
static Boolean lud(Matrix& arg, int perm[], int* swap_par, Matrix* mat);
static void lu2inv(Matrix& mat, int perm[], Matrix* invp);
// ***********************************************************************
RowMatrix::RowMatrix() {
columns = 0;
rmr = NULL;
}
// ***********************************************************************
RowMatrix::RowMatrix(RowMatrix &m) {
columns = m.Columns();
if (columns > MAX_LOCAL_DIMEN) {
rmr = new Scalar[columns];
} else {
rmr = local;
}
for (int c = 0; c < columns; c++) {
rmr[c] = m[c];
}
}
// ***********************************************************************
RowMatrix& RowMatrix::operator=(RowMatrix &m)
{
if ((rmr != NULL) && (columns > MAX_LOCAL_DIMEN)) {
delete rmr;
}
columns = m.Columns();
if (columns > MAX_LOCAL_DIMEN) {
rmr = new Scalar[columns];
} else {
rmr = local;
}
for (int c = 0; c < columns; c++) {
rmr[c] = m[c];
}
return (*this);
}
// ***********************************************************************
RowMatrix::~RowMatrix()
{
if ((rmr != NULL) && (columns > MAX_LOCAL_DIMEN)) {
delete rmr;
}
}
// ***********************************************************************
RowMatrix::RowMatrix(int c) {
if (c < 0)
errh.ErrorExit("RowMatrix::RowMatrix(int c)",
"c is negative",
ErrVal("c = ", c));
columns = c;
if (c > MAX_LOCAL_DIMEN) {
rmr = new Scalar[c];
} else {
rmr = local;
}
}
// ***********************************************************************
RowMatrix::RowMatrix(Scalar s0) {
columns = 1;
rmr = local;
rmr[0] = s0;
}
// ***********************************************************************
RowMatrix::RowMatrix(Scalar s0, Scalar s1) {
columns = 2;
rmr = local;
rmr[0] = s0;
rmr[1] = s1;
}
// ***********************************************************************
RowMatrix::RowMatrix(Scalar s0, Scalar s1, Scalar s2) {
columns = 3;
rmr = local;
rmr[0] = s0;
rmr[1] = s1;
rmr[2] = s2;
}
// ***********************************************************************
RowMatrix::RowMatrix(Scalar s0, Scalar s1, Scalar s2, Scalar s3) {
columns = 4;
rmr = local;
rmr[0] = s0;
rmr[1] = s1;
rmr[2] = s2;
rmr[3] = s3;
}
// ***********************************************************************
Scalar& RowMatrix::operator[](int c) {
if ((c < 0) || (c >= Columns()))
errh.ErrorExit("Scalar& RowMatrix::operator[](int c)",
"c out of range",
ErrVal("c = ", c),
(*this));
return rmr[c];
}
// ***********************************************************************
RowMatrix AdjointRow(RowMatrix& rmat, int comit) {
int c = rmat.Columns();
int rc = ((comit >= 0) && (comit < c)) ? c - 1 : c;
RowMatrix result(c - 1);
int i = 0, j = 0;
while (i < c) {
if (i != comit) {
result[j] = rmat[i];
j++;
}
i++;
}
return result;
}
// ***********************************************************************
void RowMatrix::debug_out(ostream& os, int indent) {
char* iblock = new char[indent + 1];
for(int i = 0; i < indent; i++)
*(iblock + i) = SPACE_CHAR;
*(iblock + i) = NULL_CHAR;
int c = Columns();
os << iblock << c << " column RowMatrix\n";
os << iblock << OPENBRACKET_STRING << " ";
for(i = 0; i < c; i++)
#ifdef c_plusplus
os << form("%12.5f", (*this)[i]); // cfront
#else
os.form("%12.5f", (*this)[i]);
#endif
os << " " << CLOSEBRACKET_STRING << "\n";
delete iblock;
}
// ***********************************************************************
ostream& operator<<(ostream& os, RowMatrix& rmat) {
int c = rmat.Columns();
os << OPENBRACKET_STRING << " ";
for(int i = 0; i < c; i++)
os << "\t" << rmat[i];
os << " " << CLOSEBRACKET_STRING;
return os;
}
// ***********************************************************************
// ***********************************************************************
Matrix::Matrix() {
is_identity = FALSE;
rows = 0;
columns = 0;
mr = NULL;
}
// ***********************************************************************
Matrix::Matrix(Matrix &m) {
Boolean mi = m.Is_Identity();
rows = m.Rows();
columns = m.Columns();
if (rows > MAX_LOCAL_DIMEN) {
mr = new RowMatrix[rows];
} else {
mr = local;
}
for(int k = 0; k < rows; k++)
mr[k] = RowMatrix(columns);
for(int i = 0; i < rows; i++)
for(int j = 0; j < columns; j++)
mr[i][j] = m[i][j];
is_identity = mi;
if (mi) m.Set_Identity(TRUE);
}
// ***********************************************************************
Matrix& Matrix::operator=(Matrix &m) {
// If something is already assigned to this matrix, we may need to
// release free store before assigning a new matrix. If the matrix
// is using free store to hold the row matrices, deleting the
// row matrices will call the RowMatrix destructor automatically,
// releasing any free store used by the RowMatrices. If this matrix
// is using local store to hold row matrices, and those row matrices
// are using free store, that free store will either get released
// when the row matrix gets a new assignment, OR when this matrix
// is finally destroyed.
if ((mr != NULL) && (rows > MAX_LOCAL_DIMEN)) {
delete [rows] mr;
}
Boolean mi = m.Is_Identity();
rows = m.Rows();
columns = m.Columns();
if (rows > MAX_LOCAL_DIMEN) {
mr = new RowMatrix[rows];
} else {
mr = local;
}
for(int k = 0; k < rows; k++)
mr[k] = RowMatrix(columns);
for(int i = 0; i < rows; i++)
for(int j = 0; j < columns; j++)
mr[i][j] = m[i][j];
is_identity = mi;
if (mi) m.Set_Identity(TRUE);
return *this;
}
// ***********************************************************************
Matrix::~Matrix()
{
if ((mr != NULL) && (rows > MAX_LOCAL_DIMEN)) {
delete [rows] mr;
}
}
// ***********************************************************************
Matrix::Matrix(int r, int c) {
if ((r < 0) || (c < 0))
errh.ErrorExit("Matrix::Matrix(int r, int c)",
"r or c negative",
ErrVal("r = ", r),
ErrVal("c = ", c));
is_identity = FALSE;
rows = r;
columns = c;
if (rows > MAX_LOCAL_DIMEN) {
mr = new RowMatrix[r];
} else {
mr = local;
}
for(int i = 0; i < r; i++)
mr[i] = RowMatrix(c);
}
// ***********************************************************************
Matrix::Matrix(int dim) {
if (dim < 0)
errh.ErrorExit("Matrix::Matrix(int dim)",
"dim is negative",
ErrVal("dim = ", dim));
is_identity = FALSE;
rows = columns = dim;
if (rows > MAX_LOCAL_DIMEN) {
mr = new RowMatrix[rows];
} else {
mr = local;
}
for(int i = 0; i < dim; i++)
mr[i] = RowMatrix(dim);
}
// ***********************************************************************
Matrix::Matrix(RowMatrix& rm0) {
is_identity = FALSE;
rows = 1;
mr = local;
mr[0] = rm0;
columns = mr[0].Columns();
}
// ***********************************************************************
Matrix::Matrix(RowMatrix& rm0, RowMatrix& rm1) {
is_identity = FALSE;
rows = 2;
mr = local;
mr[0] = rm0;
mr[1] = rm1;
columns = mr[0].Columns();
if (columns != mr[1].Columns()) {
errh.ErrorExit("Matrix::Matrix(RowMatrix&, RowMatrix&)",
"Not all row matrices have same column count",
(*this));
}
}
// ***********************************************************************
Matrix::Matrix(RowMatrix& rm0, RowMatrix& rm1, RowMatrix& rm2) {
is_identity = FALSE;
rows = 3;
mr = local;
mr[0] = rm0;
mr[1] = rm1;
mr[2] = rm2;
columns = mr[0].Columns();
for(int i = 1; i < rows; i++) {
if (columns != mr[i].Columns()) {
errh.ErrorExit("Matrix::Matrix(RowMatrix&, RowMatrix&)",
"Not all row matrices have same column count",
(*this));
}
}
}
// ***********************************************************************
Matrix::Matrix(RowMatrix& rm0, RowMatrix& rm1, RowMatrix& rm2, RowMatrix& rm3)
{
is_identity = FALSE;
rows = 4;
mr = local;
mr[0] = rm0;
mr[1] = rm1;
mr[2] = rm2;
mr[3] = rm3;
columns = mr[0].Columns();
for(int i = 1; i < rows; i++) {
if (columns != mr[i].Columns()) {
errh.ErrorExit("Matrix::Matrix(RowMatrix&, RowMatrix&)",
"Not all row matrices have same column count",
(*this));
}
}
}
// ***********************************************************************
Matrix ZeroMatrix(int r, int c) {
Matrix result(r, c);
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
result[i][j] = 0;
result.Set_Identity(FALSE);
return result;
}
// ***********************************************************************
Matrix IdentityMatrix(int dim) {
Matrix result = ZeroMatrix(dim);
for(int i = 0; i < dim; i++)
result[i][i] = 1;
result.Set_Identity(TRUE);
return result;
}
// ***********************************************************************
RowMatrix& Matrix::operator[](int r) {
if ((r < 0) || (r >= Rows()))
errh.ErrorExit("RowMatrix& Matrix::operator[](int r)",
"r out of range",
ErrVal("r = ", r),
(*this));
// Since we are handing out a ref into the matrix, we cannot guarantee
// it is an identity anymore:
is_identity = FALSE;
return mr[r];
}
// ***********************************************************************
Matrix Adjoint(Matrix& mat, int romit, int comit) {
int r = mat.Rows();
int c = mat.Columns();
int rr = ((romit >= 0) && (romit < r)) ? r - 1 : r;
int rc = ((comit >= 0) && (comit < c)) ? c - 1 : c;
Matrix result(rr, rc);
int i = 0, j = 0;
while (i < r) {
if (i != romit) {
result[j] = AdjointRow(mat[i], comit);
j++;
}
i++;
}
result.Set_Identity(FALSE);
return result;
}
// ***********************************************************************
Matrix Transpose(Matrix& mat) {
int r = mat.Columns();
int c = mat.Rows();
if (mat.Is_Identity()) {
return mat;
}
Matrix result(r, c);
result.Set_Identity(FALSE);
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
result[i][j] = mat[j][i];
return result;
}
// ***********************************************************************
Matrix operator+(Matrix& mat1, Matrix& mat2) {
Boolean m1i = mat1.Is_Identity();
Boolean m2i = mat2.Is_Identity();
int r1 = mat1.Rows();
int r2 = mat2.Rows();
int c1 = mat1.Columns();
int c2 = mat2.Columns();
if ((c1 != c2) || (r1 != r2))
errh.ErrorExit("Matrix operator+(Matrix& mat1, Matrix& mat2)",
"matrices are not of the same size",
mat1,
mat2);
Matrix result(r1, c1);
result.Set_Identity(FALSE);
for(int i = 0; i < r1; i++)
for(int j = 0; j < c1; j++)
result[i][j] = mat1[i][j] + mat2[i][j];
if (m1i) mat1.Set_Identity(TRUE);
if (m2i) mat2.Set_Identity(TRUE);
return result;
}
// ***********************************************************************
Matrix operator-(Matrix& mat1, Matrix& mat2) {
Boolean m1i = mat1.Is_Identity();
Boolean m2i = mat2.Is_Identity();
int r1 = mat1.Rows();
int r2 = mat2.Rows();
int c1 = mat1.Columns();
int c2 = mat2.Columns();
if ((c1 != c2) || (r1 != r2))
errh.ErrorExit("Matrix operator-(Matrix& mat1, Matrix& mat2)",
"matrices are not of the same size",
mat1,
mat2);
Matrix result = Matrix(r1, c1);
result.Set_Identity(FALSE);
for(int i = 0; i < r1; i++)
for(int j = 0; j < c1; j++)
result[i][j] = mat1[i][j] - mat2[i][j];
if (m1i) mat1.Set_Identity(TRUE);
if (m2i) mat2.Set_Identity(TRUE);
return result;
}
// ***********************************************************************
Matrix operator-(Matrix& mat) {
Boolean mi = mat.Is_Identity();
int r = mat.Rows();
int c = mat.Columns();
Matrix result = Matrix(r, c);
result.Set_Identity(FALSE);
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
result[i][j] = - mat[i][j];
if (mi) mat.Set_Identity(TRUE);
return result;
}
// ***********************************************************************
Matrix operator*(Matrix& mat1, Matrix& mat2) {
if (mat1.Is_Identity()) {
return mat2;
} else if (mat2.Is_Identity()) {
return mat1;
}
int r1 = mat1.Rows();
int r2 = mat2.Rows();
int c1 = mat1.Columns();
int c2 = mat2.Columns();
if (c1 != r2)
errh.ErrorExit("Matrix operator*(Matrix& mat1, Matrix& mat2)",
"mat1.Columns() != mat2.Rows()",
mat1,
mat2);
int rr = r1;
int rc = c2;
Matrix result(rr, rc);
result.Set_Identity(FALSE);
for(int i = 0; i < rr; i++)
for(int j = 0; j < rc; j++) {
result[i][j] = 0.0;
for(int k = 0; k < c1; k++)
result[i][j] += mat1[i][k] * mat2[k][j];
}
return result;
}
// ***********************************************************************
Matrix operator*(Matrix& mat, Scalar scal) {
Boolean mi = mat.Is_Identity();
int r = mat.Rows();
int c = mat.Columns();
Matrix result = Matrix(r, c);
result.Set_Identity(FALSE);
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
result[i][j] = scal * mat[i][j];
if (mi) mat.Set_Identity(TRUE);
return result;
}
// ***********************************************************************
Matrix operator/(Matrix& mat, Scalar scal) {
Boolean mi = mat.Is_Identity();
if (scal == 0.0)
errh.ErrorExit("Matrix operator/(Matrix& mat, Scalar scal)",
"Divide by zero",
ErrVal("scal = ", scal));
int r = mat.Rows();
int c = mat.Columns();
Matrix result(r, c);
result.Set_Identity(FALSE);
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
result[i][j] = mat[i][j] / scal;
if (mi) mat.Set_Identity(TRUE);
return result;
}
// ***********************************************************************
Matrix& Matrix::operator+=(Matrix& mat) {
int r = Rows();
int c = Columns();
is_identity = FALSE;
if ((c != mat.Columns()) || (r != mat.Rows()))
errh.ErrorExit("Matrix& Matrix::operator+=(Matrix& mat)",
"matrices are not of the same size",
(*this),
mat);
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
(*this)[i][j] += mat[i][j];
return *this;
}
// ***********************************************************************
Matrix& Matrix::operator-=(Matrix& mat) {
int r = Rows();
int c = Columns();
is_identity = FALSE;
if ((c != mat.Columns()) || (r != mat.Rows()))
errh.ErrorExit("Matrix& Matrix::operator-=(Matrix& mat)",
"matrices are not of the same size",
(*this),
mat);
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
(*this)[i][j] -= mat[i][j];
return *this;
}
// ***********************************************************************
Matrix& Matrix::operator*=(Matrix& mat) {
Matrix result;
if (is_identity) {
is_identity = mat.Is_Identity();
result = mat;
} else if (mat.Is_Identity()) {
result = *this;
} else {
is_identity = FALSE;
result = *this * mat; // hard to multiply in place
}
int r = rows = result.Rows();
int c = columns = result.Columns();
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
(*this)[i][j] = result[i][j];
return *this;
}
// ***********************************************************************
Matrix& Matrix::operator*=(Scalar scal) {
int r = Rows();
int c = Columns();
is_identity = FALSE;
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
(*this)[i][j] *= scal;
return *this;
}
// ***********************************************************************
Matrix& Matrix::operator/=(Scalar scal) {
if (scal == 0.0)
errh.ErrorExit("Matrix& Matrix::operator/=(Scalar scal)",
"divide by zero",
ErrVal("scal = ", scal));
int r = Rows();
int c = Columns();
is_identity = FALSE;
for(int i = 0; i < r; i++)
for(int j = 0; j < c; j++)
(*this)[i][j] /= scal;
return *this;
}
// ***********************************************************************
void Matrix::debug_out(ostream& os, int indent) {
char* iblock = new char[indent + 1];
for(int i = 0; i < indent; i++)
*(iblock + i) = SPACE_CHAR;
*(iblock + i) = NULL_CHAR;
int r = Rows();
os << iblock << ast;
os << iblock << OPENBRACKET_STRING << " " <<
r << " by " << Columns() << " Matrix\n";
for(i = 0; i < r; i++)
(*this)[i].debug_out(os, indent + 3);
os << iblock << CLOSEBRACKET_STRING << "\n";
os << iblock << ast;
delete iblock;
}
// ***********************************************************************
ostream& operator<<(ostream& os, Matrix& mat) {
int r = mat.Rows();
os << OPENBRACKET_STRING << " ";
for(int i = 0 ; i < (r - 1); i++)
os << "\t" << mat[i] << "\n";
os << "\t" << mat[i] << " " << CLOSEBRACKET_STRING;
return os;
}
// ***********************************************************************
//
// New determinant and inverse routines based on algorithms given in
// "Numerical Recipes in C" by Press et. al.
//
// ***********************************************************************
Scalar Det(Matrix& mat)
{
int n = mat.Rows();
Scalar retval;
//
// Check if matrix is square. Also do any quick kills:
//
if (n != mat.Columns()) {
errh.ErrorExit("Scalar Det(Matrix& mat)", "Non-square matrix", mat);
}
if (mat.Is_Identity()) {
return (1.0);
} else if (n == 1) {
return (mat[0][0]);
} else if (n == 2) {
retval = (mat[0][0] * mat[1][1]) - (mat[0][1] * mat[1][0]);
return (retval);
}
// Need to dynamically build an integer permutation array to be
// used by the decomposition routine. (Can't use an IntList,
// since they are defined at a higher level in this system):
int *perm = (int *)malloc((unsigned)(n * sizeof(int)));
//
// Use the LU decomposition routine. Equation 2.5.1 of Press et. al.
// says we just have to multiply together the diagonal elements of
// the LU decomposition. The parity of the swaps (-1 or 1) is
// found in d. If the LU routine deems the matrix to be singular,
// this routine simply returns 0.0:
//
Matrix res;
int d;
if (!lud(mat, perm, &d, &res)) {
retval = 0.0;
} else {
retval = (Scalar)d;
for (int i = 0; i < n; i++) {
retval *= res[i][i];
}
}
//
// Free up permutation array and return:
//
free((char *)perm);
return ((Scalar)retval);
}
// ***********************************************************************
//
// User should call determinant routine first to find out if matrix
// is singular.
//
Matrix Inverse(Matrix& mat)
{
int n = mat.Rows();
//
// Check if matrix is square. Also skip inversion if matrix happens
// to be an identity:
//
if (n != mat.Columns()) {
errh.ErrorExit("Matrix Inverse(Matrix& mat)", "Non-square matrix", mat);
}
if (mat.Is_Identity()) {
return mat;
}
//
// Need to dynamically build an integer permutation array to be
// used by the decomposition routine. (Can't use an IntList,
// since they are defined at a higher level in this system):
//
int *perm = (int *)malloc((unsigned)(n * sizeof(int)));
//
// Use the LU decomposition routine followed by routine that converts
// this representation into the inverse. If the LU routine deems
// the matrix to be singular (if it finds a pivot near zero) this
// routine will cause an error:
//
Matrix result, ludm;
int d;
if (!lud(mat, perm, &d, &ludm)) {
errh.ErrorExit("Matrix Inverse(Matrix& mat)", "Matrix is singular", mat);
}
lu2inv(ludm, perm, &result);
//
// Free up permutation array and return:
//
free((char *)perm);
return result;
}
// ***********************************************************************
//
// This routine does LU decomposition of a matrix using Crout's method
// with partial pivoting. It is a new implementation of the algorithm
// described in Press. It returns TRUE if successful, fills in the
// parity of the row swaps, fills in the result matrix with its
// LU decomposition, and fills in a permutation integer array such
// that perm[i] holds the integer j, which says that to find the ith row
// of the original LU, swap in the jth row of returned matrix, GIVEN
// that all previous swaps have also been carried out.
//
static Boolean lud(Matrix& arg, int perm[], int* swap_par, Matrix* mat)
{
int size = arg.Rows();
RowMatrix scales(size);
RowMatrix temp;
int row, col, i;
Scalar max, val;
//
// Initialize swap parity, and copy the argument matrix (we do NOT want
// to destroy the original matrix):
//
*swap_par = 1;
*mat = arg;
//
// The first step is to loop over all the rows to find the largest
// element of each row. This is needed for "implicit pivoting" as
// described by Press: it is used to scale the comparisons that
// are used to find the largest pivot. If we find a row with
// only (exact) zeros, report the singularity and quit:
//
for (row = 0; row < size; row++) {
max = 0.0;
for (col = 0; col < size; col++) {
val = fabs((*mat)[row][col]);
if (val > max) {
max = val;
}
}
if (max == 0.0) {
return (FALSE);
} else {
scales[row] = 1.0 / max;
}
}
//
// Now do Crout's method, which loops over the matrix column by column:
//
for (col = 0; col < size; col++) {
Scalar hold;
// First thing to do is find the guys above the diagonal, using
// eq. 2.3.12. Using a Scalar temp to hold results (as done in
// Press) cuts down on [] operator calls:
for (row = 0; row < col; row++) {
hold = (*mat)[row][col];
for (i = 0; i < row; i++) {
hold -= (*mat)[row][i] * (*mat)[i][col];
}
(*mat)[row][col] = hold;
}
// Now find the guys on and below the diagonal, using equation
// 2.3.13. As described in Press, wait before dividing through
// by the pivot, which may change:
Scalar best_yet = 0.0;
int pivot_candidate = col;
for (row = col; row < size; row++) {
hold = (*mat)[row][col];
for (i = 0; i < col; i++) {
hold -= (*mat)[row][i] * (*mat)[i][col];
}
(*mat)[row][col] = hold;
// Figure out if this new value is a better candidate for
// the pivot, and record it if it is. Uses the measure
// described by Press to implement implicit pivoting:
hold = fabs(hold) * scales[row];
if (hold >= best_yet) {
best_yet = hold;
pivot_candidate = row;
}
}
// Need to record where this row is coming from, even if there
// is no swap. If we have found a better pivot, swap the rows,
// changing the swap parity. Keep the scale information consistent,
// though we don't need the scale anymore for the current row:
perm[col] = pivot_candidate;
if (pivot_candidate != col) {
temp = (*mat)[col];
(*mat)[col] = (*mat)[pivot_candidate];
(*mat)[pivot_candidate] = temp;
scales[pivot_candidate] = scales[col];
*swap_par *= -1;
}
// Now divide the guys >>below<< the diagonal through by the
// new pivot. The diagonal element is not divided, since we
// actually want to use equation 2.3.12 for it. If a pivot
// is still very small, report the singularity and quit:
Scalar factor = (*mat)[col][col];
if (fabs(factor) < EPSILON) {
return FALSE;
}
for (row = (col + 1); row < size; row++) {
(*mat)[row][col] /= factor;
}
}
return TRUE;
}
// ***********************************************************************
//
// Given an LU decomposition of a matrix and associated permutation
// key, this routine builds the inverse matrix. It uses the
// forward/backward substitution algorithm described in Press, using
// columns of an identity matrix for right hand sides. The Matrix
// is an LU decomposition, the integer array is the permutation key,
// and the Matrix pointer indicates where to return the result.
//
static void lu2inv(Matrix& mat, int perm[], Matrix* invp)
{
int size = mat.Rows();
int row, col, k;
Boolean summing;
Scalar hold;
int first_nz;
int location;
RowMatrix temprm;
//
// Use an identity matrix to provide the right hand sides. Permute
// the rows based on the permutation key:
//
*invp = IdentityMatrix(size);
for (row = 0; row < size; row++) {
location = perm[row];
temprm = (*invp)[location];
(*invp)[location] = (*invp)[row];
(*invp)[row] = temprm;
}
//
// Loop through the columns of the rhs matrix, building the inverse
// by columns:
//
for (col = 0; col < size; col++) {
// Start with forward substitution (Press eq. 2.3.6). Use the
// optimization they describe, i.e. only start the summing when
// a non-zero value is hit. Note that division by the diagonal
// can be skipped because it always happens to be 1.0:
summing = FALSE;
first_nz = 0;
for (row = 0; row < size; row++) {
hold = (*invp)[row][col];
if (summing) {
for (k = first_nz; k < row; k++) {
hold -= mat[row][k] * (*invp)[k][col];
}
} else if (hold != 0.0) {
summing = TRUE;
first_nz = row;
}
(*invp)[row][col] = hold;
}
// Now it's time to do back substitution, which just involves
// implementing equation 2.3.7. Again, using a Scalar temp
// avoids several [] operator calls:
for (row = size - 1; row >= 0; row--) {
hold = (*invp)[row][col];
for (k = row + 1; k < size; k++) {
hold -= mat[row][k] * (*invp)[k][col];
}
(*invp)[row][col] = hold / mat[row][row];
}
}
return;
}
// ***********************************************************************